// -*- C++ -*-
/*!
 * @file  SimRangeUrg.cpp
 * @brief DataConversionRTC
 * @date $Date$
 *
 * Copyright (c) 2008, National Institute of Advanced Industrial Science and Tec                       
 * hnology (AIST). All rights reserved. This program is made available under the te                    
 * rms of the Eclipse Public License v1.0 which accompanies this distribution, and                     
 * is available at http://www.eclipse.org/legal/epl-v10.html  
 * $Id$
 */

#include "SimRangeUrg.h"

// Module specification
// <rtc-template block="module_spec">
static const char* SimRangeUrg_spec[] =
  {
    "implementation_id", "SimRangeUrg",
    "type_name",         "SimRangeUrg",
    "description",       "DataConversionRTC",
    "version",           "1.0.0",
    "vendor",            "AIST",
    "category",          "tool",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "5",
    "language",          "C++",
    "lang_type",         "compile",
     "exec_cxt.periodic.rate", "1000.0",
    // Configuration variables
    "conf.default.startAngle", "-45.0",
    "conf.default.endAngle", "225.0",
    "conf.default.groupingNumber", "1",
    "conf.default.scanAngle", "270.0",
    "conf.default.scanStep", "0.25",
    "conf.default.scanMaxDistance", "10.0",
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
SimRangeUrg::SimRangeUrg(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_InRangeIn("InRange", m_InRange),
    m_outOut("out", m_out)
    //    m_OutSensorDataOut("OutSensorData", m_OutSensorData)

    // </rtc-template>
{
}

/*!
 * @brief destructor
 */
SimRangeUrg::~SimRangeUrg()
{
}



RTC::ReturnCode_t SimRangeUrg::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("InRange", m_InRangeIn);
  
  // Set OutPort buffer
  registerOutPort("out", m_outOut);


  //  registerOutPort("OutSensorData", m_OutSensorDataOut);  
  // Set service provider to Ports
  
  // Set service consumers to Ports
  
  // Set CORBA Service Ports
  
  // </rtc-template>
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("startAngle", m_startAngle, "-45.0");
  bindParameter("endAngle", m_endAngle, "225.0");
  bindParameter("groupingNumber", m_groupingNumber, "1");
  bindParameter("scanAngle", m_scanAngle, "270.0");
  bindParameter("scanStep", m_scanStep, "0.25");
  bindParameter("scanMaxDistance", m_scanMaxDistance, "10.0");
  
  // </rtc-template>

/*--------------------Addition---------------------*/
	std::cout << "[INFO] onInitialize" << std::endl;
  // -- callback -- //
  #if DEBUG_CALLBACK
	m_InRangeIn.setOnWriteConvert(&m_CallbackRange);
  #endif
	
	// -- prot setting -- //
	std::cout.setf(std::ios_base::fixed,std::ios_base::floatfield);
	std::cout.precision(11);
/*--------------------Addition---------------------*/

  return RTC::RTC_OK;
}


RTC::ReturnCode_t SimRangeUrg::onFinalize()
{
  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t SimRangeUrg::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t SimRangeUrg::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t SimRangeUrg::onActivated(RTC::UniqueId ec_id)
{
  std::cout << "[INFO] onActivated" << std::endl;

	// -- Maintenance of configuration parameter -- //
	  m_scanAngle_= m_scanAngle;
	  m_scanStep_= m_scanStep;
	  m_scanMaxDistance_= m_scanMaxDistance;

	  std::cout << "[INFO] " << m_scanAngle_ << " "<< m_scanStep_ << " "<< m_scanMaxDistance_ << std::endl;

  // -- Debugging file open -- //
#if DEBUG_FILE
	  RangeLog.open("RangeLog.log");
#endif
	
	// -- Initialization of Execute time [old] -- //
#if DEBUG_TIME
	    Time_Execute_old= TM2TIME(GetTime());
#endif

  return RTC::RTC_OK;
}



RTC::ReturnCode_t SimRangeUrg::onDeactivated(RTC::UniqueId ec_id)
{
	std::cout << "[INFO] onDeactivated" << std::endl;
	
	// -- Debugging file close
	#if DEBUG_FILE
	RangeLog.close();
	#endif
  return RTC::RTC_OK;
}



RTC::ReturnCode_t SimRangeUrg::onExecute(RTC::UniqueId ec_id)
{
  //std::cout << "[INFO] onExecute" << std::endl;

	/*---------------------------------------------*/
	//  Get Data Port [Range]
	/*---------------------------------------------*/
	if( m_InRangeIn.isNew() ){

		m_InRangeIn.read();
		  
		  // -- check of Distance Update -- //
		  int Size = (int)m_InRange.data.length();

		  std::cout << "RangeDataSize " << Size << std::endl;

		  if( m_CheckDistance.empty() ){  m_CheckDistance.resize(Size); }

		  bool isUpdate= false;

		  for(int i=0; i<Size; i++){
		      if(m_CheckDistance[i] != m_InRange.data[i]){
					m_CheckDistance[i]= m_InRange.data[i];
					isUpdate= true;
		      }
			
		 }
		 if( !isUpdate ){ // updated?
		    std::cout << "  !isUpdate  " << Size << std::endl;
			return RTC::RTC_OK;
		 }
		 std::cout << "  Convet ....  "  << std::endl;

		// -- Conversion of Distance Data -- //
		int SensorStep= 0;
		int dataSize= (int)m_InRange.data.length();

		  //vector <int> Distance;
		vector <long> Distance;
		double startAngle= ( ( RAD2DEG(M_PI*2.0) - m_scanAngle_ ) - RAD2DEG(M_PI) ) / 2.0;

		for(int i=0; i<dataSize; i+=m_groupingNumber){

			double Angle= startAngle + i * m_scanStep_;

			if( Angle < m_startAngle || Angle > m_endAngle ){ continue; }

			if( m_InRange.data[i] > m_scanMaxDistance_ ){
			    Distance.push_back( m_scanMaxDistance_ *1000.0 ); 
			   // std::cout << "Error: Over MaxDistance " << std::endl;
			}else{
				Distance.push_back( int(m_InRange.data[i] * 1000.0) );
			}
			SensorStep++;
		}

		/*
		int Distance= 0;
		int MaxDistance= int( m_scanMaxDistance_ * 1000.0);
		int groupingRange= m_groupingNumber / 2;
		int SearchAngleCount= (int)( (m_endAngle - m_startAngle) / m_scanStep_ );
		int StartCount= dataSize / 2 - SearchAngleCount / 2;
		int EndCount= dataSize / 2 + SearchAngleCount / 2;

		for(int i=0; i<dataSize; i++){
			double Angle= startAngle + i * m_scanStep_;
			if( Angle < m_startAngle || Angle > m_endAngle ) continue;
			
			// Conversion [mm] ← [m]
			Distance+= int(m_InRange.data[i] * 1000.0);
			
			if( 0 == i % m_groupingNumber ){
			}
			// Average Distance
			Distance/= m_groupingNumber;
			// Check of Max Distance
			if( Distance > MaxDistance ) Distance= 19; // Error code
			// Set of data
			Distance.push_back( Distance );
			SensorStep++;
		}
		*/
		
		//for Invent
		/*
		// -- Set of Measured Data -- //
		m_OutSensorData.data.startPosition= m_startAngle;
		m_OutSensorData.data.endPosition= m_endAngle;
		m_OutSensorData.data.dataInterval= m_scanStep_ * m_groupingNumber;
		m_OutSensorData.data.scanInterval= m_scanStep_;
		m_OutSensorData.data.dataGroupingNumber= m_groupingNumber;
		m_OutSensorData.data.sensorState= "NORMAL";  // NORMAL or UPDATED
		m_OutSensorData.tm= m_InRange.tm;
		m_OutSensorData.data.distance.length(SensorStep);
		for(int i=0; i<SensorStep; i++){
			m_OutSensorData.data.distance[i]= Distance[i];
		}
		m_OutSensorDataOut.write();
		*/
		if(m_out.data.length() < SensorStep){
		  m_out.data.length(SensorStep);
		}
		for(int i=0; i<SensorStep; i++){
		  m_out.data[i] = Distance[i];
		}

		m_outOut.write();

	}else{
		std::cout << "[Warning] The data port is not input [Range data]" << std::endl;
	}
  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t SimRangeUrg::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t SimRangeUrg::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t SimRangeUrg::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t SimRangeUrg::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t SimRangeUrg::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/



extern "C"
{
 
  void SimRangeUrgInit(RTC::Manager* manager)
  {
    coil::Properties profile(SimRangeUrg_spec);
    manager->registerFactory(profile,
                             RTC::Create<SimRangeUrg>,
                             RTC::Delete<SimRangeUrg>);
  }
  
};


